#ifndef TREE_H
#define TREE_H

#include <list>
#include <math.h>
#include <stdarg.h>
#include <utility>

template <
        int _dim_,
        int _maxCildElementCount_,
        typename _ElementType_,
        typename _ElementPositionType_>
class Tree
{
public:
    _ElementPositionType_ rangeMax[_dim_];
    _ElementPositionType_ rangeMin[_dim_];
    std::list<std::pair<_ElementType_*,_ElementPositionType_*>> elements;
    Tree<_dim_, _maxCildElementCount_, _ElementType_, _ElementPositionType_> *child[1 << _dim_];
    Tree()
    {
        for(auto &_ch : child)
            _ch = nullptr;
    }
private:
    int getIndex(const _ElementPositionType_ *pos)
    {
        if(this->child[0]!=nullptr)
        {
            int _index = 0;
            for(int _d=0;_d<_dim_;++_d)
            {
                if(pos[_d] >= rangeMin[_d] + (rangeMax[_d]-rangeMin[_d])/2)
                    _index += 1 << _d;
            }
            return _index;
        }
        return -1;
    }
public:
    void addElement(_ElementType_* el, _ElementPositionType_ *pos)
    {
        int _index = getIndex(pos);
        if(_index != -1)
            child[_index]->addElement(el, pos);
        else
        {
            std::pair<_ElementType_*,_ElementPositionType_*> _p(el,pos);
            this->elements.push_back(_p);
            // put here self-reference if need
            if(elements.size() > _maxCildElementCount_ && child[0]==nullptr)
            {
                int _i = 0;
                for(auto &_ch : child)
                {
                    _ch = new Tree();
                    for(int _d=0;_d<_dim_;++_d)
                    {
                        _ch->rangeMin[_d] =
                            (1-((_i & 1<<_d)>>_d))*(rangeMin[_d]) +
                            ((_i & 1<<_d)>>_d)*(rangeMin[_d] + (rangeMax[_d]-rangeMin[_d])/2);
                        _ch->rangeMax[_d] =
                            (1-((_i & 1<<_d)>>_d))*(rangeMin[_d] + (rangeMax[_d]-rangeMin[_d])/2) +
                            ((_i & 1<<_d)>>_d)*(rangeMax[_d]);
                    }
                    ++_i;
                }
                for(auto _elPair = elements.begin();
                    _elPair != elements.end(); )
                {
                    _index = getIndex((*_elPair).second);
                    //if(index != -1) // used in case of not empty parent nodes
                    //{
                        std::pair<_ElementType_*,_ElementPositionType_*> _tmpP(*_elPair);
                        // destroy here self-reference if need
                        _elPair = elements.erase(_elPair);
                        child[_index]->addElement(_tmpP.first,_tmpP.second);
                    //}
                    //else ++_elPair;
                }
            }
        }
    }
/*public:
    void deleteElement(Triangle *tr)
    {
        int index = this->GetIndex(tr);
        if(index != -1)
            descendant[index]->DeleteTriangle(tr);
        else
            this->triangles.erase(tr->itsMeQuadTree);
    }*/
public:
    std::list<std::pair<_ElementType_*,_ElementPositionType_*>>
        getElements(const _ElementPositionType_ *pos)
    {
        std::list<std::pair<_ElementType_*,_ElementPositionType_*>> _curList;
        _curList.insert(
                    _curList.end(),
                    this->elements.begin(),
                    this->elements.end());
        int _index = this->getIndex(pos);
        if(_index != -1)
        {
            std::list<std::pair<_ElementType_*,_ElementPositionType_*>> _childList =
                    child[_index]->getElements(pos);
            _curList.insert(
                        _curList.end(),
                        _childList.begin(),
                        _childList.end());
        }
        return _curList;
    }
public:
    void clear()
    {
        if(child[0]!=NULL)
        {
            for(auto &_ch : child)
            {
                _ch->clear();
                delete(_ch);
                _ch = nullptr;
            }
        }
        elements.clear();
    }
public: ~Tree()
    {
        clear();
    }
};
template<int _maxCildElementCount_, typename _ElementType_, typename _ElementPositionType_>
    using BinaryTree = Tree <1,_maxCildElementCount_, _ElementType_, _ElementPositionType_>;

template<int _maxCildElementCount_, typename _ElementType_, typename _ElementPositionType_>
    using QuadTree = Tree <2,_maxCildElementCount_, _ElementType_, _ElementPositionType_>;

template<int _maxCildElementCount_, typename _ElementType_, typename _ElementPositionType_>
    using OcTree = Tree <3,_maxCildElementCount_, _ElementType_, _ElementPositionType_>;
#endif // TREE_H
